#include "RigidSurface.h"

RigidSurface::RigidSurface(void)
{
}

RigidSurface::RigidSurface( SurfTrack* surf )
{
	this->surf = surf;

	surf->improve_mesh();
	surf->rebuild_static_broad_phase();

	surf->m_newpositions = surf->m_positions;

	surf->generate_new_midpoint = [&](unsigned int edgeid,
		const eltopo2d::SurfTrack & surface,
		Vec2d & result) {
			Vec2ui edge = surface.m_mesh.edges[edgeid];
			Vec2d a = surface.m_positions[edge[0]];
			Vec2d b = surface.m_positions[edge[1]];
			result = 0.5*(a+b);
	};

	eps = 0.03;
	
	//surf->improve_mesh();
	//surf->improve_mesh();
	//surf->improve_mesh();
	//surf->rebuild_static_broad_phase();	
}


RigidSurface::~RigidSurface(void)
{
}

void RigidSurface::advect( CrackList* crack, double dt )
{
	this->crack = crack;
	this->dt = dt;

	for (int i=0; i<crack->size(); i++)
	{
		crack->at(i)->active = true;
		crack->at(i)->max_t = 0;
	}

	crackIntegration();

	//surf->m_mesh.update_connectivity(surf->m_positions.size());

	//surf->improve_mesh();

	//surf->rebuild_static_broad_phase();

	//surf->rebuild_continuous_broad_phase();

	//surf->topology_changes();
}

void RigidSurface::crackCollisionPreprocessing()
{
	for (int i=0; i<crack->size(); i++)
	{
		Crack2d* cc = crack->at(i);

		int e1 = surf->m_mesh.get_incoming_edge(cc->pt_i);
		int e2 = surf->m_mesh.get_outgoing_edge(cc->pt_i);

		Vec2ui ei1 = surf->m_mesh.edges.at(e1);
		Vec2ui ei2 = surf->m_mesh.edges.at(e2);

		Vec2d l1 = surf->m_positions.at(ei1[1]) - surf->m_positions.at(ei1[0]);
		Vec2d l2 = surf->m_positions.at(ei2[1]) - surf->m_positions.at(ei2[0]);

		if (checkInsideHalfPlane(l1, cc->vel) == false || checkInsideHalfPlane(l2, cc->vel) == false)
		{
			crack->erase(crack->begin() + i);
			i--;
		}

	}
}

bool RigidSurface::checkInsideHalfPlane( Vec2d cut, Vec2d dir )
{
	Vec3d t1(cut[0], cut[1], 0);
	Vec3d t2(dir[0], dir[1], 0);
	Vec3d tz(0, 0, -1);

	Vec3d tn = cross(t1, tz);

	double check = dot(t2, tn);

	if (check < 0)
	{
		return true;
	}else{
		return false;
	}
}

void RigidSurface::crackCollisionDetection()
{
	pcs = new PotentialCollisionList();

	for (int i=0; i<crack->size(); i++)
	{
		for (int j=i+1; j<crack->size(); j++)
		{

			Vec2d as = surf->m_positions.at(crack->at(i)->pt_i);
			Vec2d ae = as + crack->at(i)->vel * dt;

			Vec2d bs = surf->m_positions.at(crack->at(j)->pt_i);
			Vec2d be = bs + crack->at(j)->vel * dt;

			Vec2d collision_t = edgeCollisionDetection(as, ae, bs, be);

			if (abs(collision_t[0] - FAIL) < 1e-8)
			{
				continue;
			}else{
				PotentialCollision pc1, pc2;

				pc1.a_i = i;
				pc1.b_i = j;

				double a_v = mag(crack->at(i)->vel);
				double b_v = mag(crack->at(j)->vel);

				pc1.a_time = collision_t[0] / a_v;
				pc1.b_time = collision_t[1] / b_v;

				pcs->push_back(pc1);

				pc2.a_i = j;
				pc2.b_i = i;

				pc2.a_time = pc1.b_time;
				pc2.b_time = pc1.a_time;

				pcs->push_back(pc2);
			}

		}
	}

	for (int i=0; i<crack->size(); i++)
	{
		Vec2d ls = surf->m_positions.at(crack->at(i)->pt_i);
		Vec2d le = ls + crack->at(i)->vel * dt;

		Vec2d query_low(min(ls[0], le[0]), min(ls[1], le[1]));
		Vec2d query_high(max(ls[0], le[0]), max(ls[1], le[1]));

		query_low -= Vec2d(0.1);
		query_high += Vec2d(0.1);

		vector<unsigned int> pc_edges;

		surf->m_broad_phase->get_potential_edge_collisions( query_low, query_high, pc_edges );

		for (int j=0; j<pc_edges.size(); j++)
		{
			Vec2d as = surf->m_positions.at(crack->at(i)->pt_i);
			Vec2d ae = as + crack->at(i)->vel * dt;

			Vec2ui pe = surf->m_mesh.edges.at(pc_edges.at(j));

			Vec2d bs = surf->m_positions.at(pe[0]);
			Vec2d be = surf->m_positions.at(pe[1]);

			if (crack->at(i)->pt_i == pe[0] || crack->at(i)->pt_i == pe[1])
			{
				continue;
			}

			Vec2d collision_t = edgeCollisionDetection(as, ae, bs, be);

			if (abs(collision_t[0] - FAIL) < 1e-8)
			{
				continue;
			}else{
				PotentialCollision pc;

				pc.a_i = i;
				pc.b_i = -1;

				double a_v = mag(crack->at(i)->vel);				

				pc.a_time = collision_t[0] / a_v;				

				pcs->push_back(pc);

				//printf("%f\n", collision_t[0]);
			}
		}

		//printf("%d\n", pc_edges.size());

	}

	printf("%d\n", pcs->size());
	
}

void RigidSurface::crackCollisionProcessing()
{
	sort(pcs->begin(), pcs->end(), sortByTime);

	for (int i=0; i<pcs->size(); i++)
	{
		PotentialCollision pc = pcs->at(i);

		printf("%f\n", pc.a_time);
	}

	for (int i=0; i<pcs->size(); i++)
	{
		PotentialCollision pc = pcs->at(i);

		if (pc.b_i == -1)
		{
			if (crack->at(pc.a_i)->active)
			{
				crack->at(pc.a_i)->max_t = pc.a_time;
				crack->at(pc.a_i)->active = false;
				crack->at(pc.a_i)->st_i = -1;
			}

			continue;
		}

		Crack2d* ca = crack->at(pc.a_i);
		Crack2d* cb = crack->at(pc.b_i);

		if (ca->active == false)
		{
			continue;
		}

		crack->at(pc.a_i)->max_t = pc.a_time;
		crack->at(pc.a_i)->st_i = pc.b_i;

		if (crack->at(pc.b_i)->max_t >= pc.b_time)
		{			
			crack->at(pc.a_i)->active = false;
		}

	}

	for (int i=0; i<crack->size(); i++)
	{
		if (crack->at(i)->active)
		{
			crack->at(i)->max_t = dt;
			crack->at(i)->active = false;
			crack->at(i)->st_i = -1;
		}
	}
}

Vec2d RigidSurface::edgeCollisionDetection( Vec2d as, Vec2d ae, Vec2d bs, Vec2d be )
{
	Vec2d adir = ae - as;
	Vec2d bdir = be - bs;

	double at = mag(adir);
	double bt = mag(bdir);

	adir /= at;
	bdir /= bt;

	Mat33d dMat(adir[0], -1 * bdir[0], 0, adir[1], -1 * bdir[1], 0, 0, 0, 1);

	Vec3d ct = inverse(dMat) * (Vec3d(bs[0], bs[1], 0) - Vec3d(as[0], as[1], 0));

	Vec2d result(FAIL, FAIL);

	if ((ct[0] >= at && ct[0] <= 0) || (ct[0] <= at && ct[0] >= 0))
	{
		if ((ct[1] >= bt && ct[1] <= 0) || (ct[1] <= bt && ct[1] >= 0))
		{
			result = Vec2d(ct[0], ct[1]);
		}
	}

	return result;
}

void RigidSurface::crackDirectionEvolution()
{
	for (int i=0; i<crack->size(); i++)
	{
		Crack2d* ck = crack->at(i);

		crackThickening(ck->pt_i, ck->vel * ck->max_t);
	}	
}

void RigidSurface::crackIntegration()
{
	crackCollisionPreprocessing();

	crackCollisionDetection();

	printf("collision size %d\n", pcs->size());

	crackCollisionProcessing();

	pcs->clear();

	delete pcs;

	crackGeneration();

	//crackDirectionEvolution();
}

void RigidSurface::crackThickening( int pt_i, Vec2d direction )
{
	double epsilon = eps;
	int index = pt_i;
	Vec2d dir = direction * 0.96;

	printf("%d\n", index);

	int ei_i = surf->m_mesh.get_incoming_edge(index);
	int eo_i = surf->m_mesh.get_outgoing_edge(index);

	Vec2ui ei = surf->m_mesh.edges.at(ei_i);
	Vec2ui eo = surf->m_mesh.edges.at(eo_i);

	Vec2d vi = surf->m_positions.at(ei[0]);
	Vec2d vo = surf->m_positions.at(eo[1]);

	Vec2d pos = surf->m_positions.at(index);	

	Vec2d vib = epsilon * vi + (1 - epsilon) * pos;
	Vec2d vob = epsilon * vo + (1 - epsilon) * pos;

	Vec2d vie = vib + dir;
	Vec2d voe = vob + dir;	

	surf->m_positions[index] += dir * 1;

	surf->remove_edge(ei_i);
	surf->remove_edge(eo_i);

	int vib_i = surf->add_vertex(vib, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);
	int vob_i = surf->add_vertex(vob, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);
	int vie_i = surf->add_vertex(vie, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);
	int voe_i = surf->add_vertex(voe, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);

	surf->add_edge(Vec2ui(ei[0], vib_i));
	surf->add_edge(Vec2ui(vib_i, vie_i));
	surf->add_edge(Vec2ui(vie_i, index));
	surf->add_edge(Vec2ui(index, voe_i));
	surf->add_edge(Vec2ui(voe_i, vob_i));	
	surf->add_edge(Vec2ui(vob_i, eo[1]));	
}

void RigidSurface::crackGeneration()
{
	//sort(crack->begin(), crack->end(), sortByMTime);

	vector<int>* crackGraph = new vector<int>();

	for (int i=0; i<crack->size(); i++)
	{
		crackGraph->push_back(i);
	}

	while (crackGraph->size() > 0)
	{
		int index = crackGraph->at(0);

		int candidate = index;

		while (true)
		{
			Crack2d* ck = crack->at(candidate);

			if (crack->at(candidate)->st_i < 0 || crack->at(crack->at(candidate)->st_i)->active)
			{
				break;
			}else{
				candidate = crack->at(candidate)->st_i;
			}
		}

		Crack2d* ck = crack->at(candidate);

		crack->at(candidate)->active = true;

		if (abs(ck->max_t - dt) < 1e-8)
		{
			crackCut(ck->pt_i, ck->vel * ck->max_t);
		}else{
			crackMerge(ck->pt_i, ck->vel * dt);
		}

		for (int i=0; i<crackGraph->size(); i++)
		{
			if (crackGraph->at(i) == candidate)
			{
				crackGraph->erase(crackGraph->begin() + i);
				break;
			}
		}
	}

	/*for (int i=crack->size()-1; i>=0; i--)
	{
		Crack2d ck = crack->at(i);

		if (abs(ck.max_t - dt) < 1e-8)
		{
			crackCut(ck.pt_i, ck.vel * ck.max_t);
		}else{
			crackMerge(ck.pt_i, ck.vel * dt);
		}

		//surf->m_mesh.update_connectivity(surf->m_positions.size());
	}*/

	//surf->improve_mesh();

	//surf->m_mesh.check_orientation();
}

void RigidSurface::crackCut( int pt_i, Vec2d direction )
{
	double epsilon = eps;
	int index = pt_i;
	Vec2d dir = direction * 0.96;

	printf("%d\n", index);

	int ei_i = surf->m_mesh.get_incoming_edge(index);
	int eo_i = surf->m_mesh.get_outgoing_edge(index);

	Vec2ui ei = surf->m_mesh.edges.at(ei_i);
	Vec2ui eo = surf->m_mesh.edges.at(eo_i);

	Vec2d vi = surf->m_positions.at(ei[0]);
	Vec2d vo = surf->m_positions.at(eo[1]);

	Vec2d pos = surf->m_positions.at(index);

	double il = mag(vi - pos);
	double ol = mag(vo - pos);

	Vec2d vib = epsilon / il * vi + (1 - epsilon / il) * pos;
	Vec2d vob = epsilon / ol * vo + (1 - epsilon / ol) * pos;

	Vec2d vie = vib + dir;
	Vec2d voe = vob + dir;	

	surf->m_positions[index] += dir * 1;

	surf->remove_edge(ei_i);
	surf->remove_edge(eo_i);

	int vib_i = surf->add_vertex(vib, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);
	int vob_i = surf->add_vertex(vob, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);
	int vie_i = surf->add_vertex(vie, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);
	int voe_i = surf->add_vertex(voe, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[index]);

	surf->add_edge(Vec2ui(ei[0], vib_i));
	surf->add_edge(Vec2ui(vib_i, vie_i));
	surf->add_edge(Vec2ui(vie_i, index));
	surf->add_edge(Vec2ui(index, voe_i));
	surf->add_edge(Vec2ui(voe_i, vob_i));	
	surf->add_edge(Vec2ui(vob_i, eo[1]));
}

void RigidSurface::crackMerge( int pt_i, Vec2d dir )
{
	Vec2d ls = surf->m_positions.at(pt_i);
	Vec2d le = ls + dir;

	Vec2d query_low(min(ls[0], le[0]), min(ls[1], le[1]));
	Vec2d query_high(max(ls[0], le[0]), max(ls[1], le[1]));

	query_low -= Vec2d(0.1);
	query_high += Vec2d(0.1);

	vector<unsigned int> pc_edges;

	surf->m_broad_phase->get_potential_edge_collisions( query_low, query_high, pc_edges );

	double max_t = 1e8;

	int e_i;

	double offset;

	for (int j=0; j<pc_edges.size(); j++)
	{
		Vec2d as = ls;
		Vec2d ae = le;

		Vec2ui pe = surf->m_mesh.edges.at(pc_edges.at(j));

		Vec2d bs = surf->m_positions.at(pe[0]);
		Vec2d be = surf->m_positions.at(pe[1]);

		if (pt_i == pe[0] || pt_i == pe[1])
		{
			continue;
		}

		Vec2d collision_t = edgeCollisionDetection(as, ae, bs, be);

		if (max_t > collision_t[0])
		{
			max_t = collision_t[0];

			e_i = pc_edges.at(j);

			offset = collision_t[1];
		}		
	}

	Vec2ui pe = surf->m_mesh.edges.at(e_i);

	Vec2d bs = surf->m_positions.at(pe[0]);
	Vec2d be = surf->m_positions.at(pe[1]);

	Vec2d b_dir = be - bs;

	double b_l = mag(b_dir);

	b_dir /= b_l;

	Vec2d e_a, e_b;

	int vib_i, vob_i, vie_i, voe_i;

	if (offset < eps)
	{
		e_a = bs;
		voe_i = pe[0];
	}else{
		e_a = bs + b_dir * (offset - eps);
		voe_i = surf->add_vertex(e_a, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[pt_i]);
		surf->add_edge(Vec2ui(pe[0], voe_i));
	}

	if (offset > b_l - eps)
	{
		e_b = be;
		vie_i = pe[1];
	}else{
		e_b = bs + b_dir * (offset + eps);
		vie_i = surf->add_vertex(e_b, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[pt_i]);
		surf->add_edge(Vec2ui(vie_i, pe[1]));
	}

	int ei_i = surf->m_mesh.get_incoming_edge(pt_i);
	int eo_i = surf->m_mesh.get_outgoing_edge(pt_i);

	Vec2ui ei = surf->m_mesh.edges.at(ei_i);
	Vec2ui eo = surf->m_mesh.edges.at(eo_i);

	Vec2d vi = surf->m_positions.at(ei[0]);
	Vec2d vo = surf->m_positions.at(eo[1]);

	Vec2d pos = surf->m_positions.at(pt_i);	

	Vec2d vic = vi - pos;
	Vec2d voc = vo - pos;

	double il = mag(vi - pos);
	double ol = mag(vo - pos);

	//Vec3d vic3(vic[0], vic[1], 0);
	//Vec3d voc3(voc[0], voc[1], 0);

	Vec3d zc(0, 0, -1);

	Vec3d zic = cross(zc, Vec3d(dir[0], dir[1], 0));
	Vec3d zoc = -1.0 * zic;

	Vec2d vib, vob;

	if (dot(vic, Vec2d(zic[0], zic[1])))
	{
		vib = eps / il * vi + (1 - eps / il) * pos;
		surf->remove_edge(ei_i);
		vib_i = surf->add_vertex(vib, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[pt_i]);
		surf->add_edge(Vec2ui(ei[0], vib_i));
	}else{
		vib = pos;
		vib_i = pt_i;
	}

	if (dot(voc, Vec2d(zoc[0], zoc[1])))
	{
		vob = eps / ol * vo + (1 - eps / ol) * pos;
		surf->remove_edge(eo_i);
		vob_i = surf->add_vertex(vob, Vec2d(0, 0), 1, Vec2d(0, 0), surf->m_volume_ids[pt_i]);
		surf->add_edge(Vec2ui(vob_i, eo[1]));
	}else{
		vob = pos;
	}	

	Vec2d vie = e_b;
	Vec2d voe = e_a;

	surf->remove_edge(e_i);

	surf->add_edge(Vec2ui(vib_i, vie_i));	
	surf->add_edge(Vec2ui(voe_i, vob_i));

	surf->remove_vertex(pt_i);

}

bool sortByTime( const PotentialCollision &lhs, const PotentialCollision &rhs )
{
	return lhs.a_time < rhs.a_time;
}

bool sortByMTime( const Crack2d &lhs, const Crack2d &rhs )
{
	return lhs.max_t < rhs.max_t;
}
